#include "modules/drivers/lidar/orin_hesai/hesai_utils/socket_input.h"

#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <poll.h>
#include <sys/file.h>
#include <sys/socket.h>
#include <unistd.h>

#include <chrono>

#include "modules/drivers/lidar/orin_hesai/common/make_time.h"

namespace {
uint64_t GetPacketTimestamp(
    const apollo::drivers::lidar_hesai::HesaiLidarPacketData &buf) {
  static constexpr int T_ID = 1065;  // Time data index of packet data
  static uint8_t utc_time[6];
  utc_time[0] = buf[T_ID] & 0xff;
  utc_time[1] = buf[T_ID + 1] & 0xff;
  utc_time[2] = buf[T_ID + 2] & 0xff;
  utc_time[3] = buf[T_ID + 3] & 0xff;
  utc_time[4] = buf[T_ID + 4] & 0xff;
  utc_time[5] = buf[T_ID + 5] & 0xff;
  uint64_t timestamp = (buf[T_ID + 6] & 0xff) | (buf[T_ID + 7] & 0xff) << 8 |
                       (buf[T_ID + 8] & 0xff) << 16 |
                       (buf[T_ID + 9] & 0xff) << 24;
  timestamp *= 1000;  // us to ns
  static struct tm tTm;
  tTm.tm_year = utc_time[0] + 100;
  if (tTm.tm_year >= 200) {
    tTm.tm_year -= 100;
  }
  tTm.tm_mon = utc_time[1] - 1;
  tTm.tm_mday = utc_time[2];
  tTm.tm_hour = utc_time[3];
  tTm.tm_min = utc_time[4];
  tTm.tm_sec = utc_time[5];
  tTm.tm_isdst = 0;
  double unix_second = static_cast<double>(
      apollo::drivers::lidar_hesai::MakeTime::Instance()->mktime_kernel(&tTm));
  timestamp += static_cast<uint64_t>(unix_second) * 1e9;
  return timestamp;
}
}  // namespace

namespace apollo {
namespace drivers {
namespace lidar_hesai {
bool SocketInput::Init(uint32_t port, size_t firing_data_packet_size) {
  firing_data_packet_size_ = firing_data_packet_size;
  if (sockfd_ != -1) {
    (void)close(sockfd_);
  }
  // connect to Lidar UDP port
  AINFO << "Opening UDP socket: port " << uint16_t(port);
  port_ = port;
  sockfd_ = socket(AF_INET, SOCK_DGRAM, 0);
  if (sockfd_ == -1) {
    AERROR << "Init socket failed, UDP port is " << port;
    return false;
  }

  sockaddr_in my_addr;                       // my address information
  memset(&my_addr, 0, sizeof(my_addr));      // initialize to zeros
  my_addr.sin_family = AF_INET;              // host byte order
  my_addr.sin_port = htons(uint16_t(port));  // short, in network byte order
  my_addr.sin_addr.s_addr = INADDR_ANY;      // automatically fill in my IP
  AINFO << "SocketInput::init " << my_addr.sin_addr.s_addr;

  const int opt = -1;
  int rtn =
      setsockopt(sockfd_, SOL_SOCKET, SO_REUSEPORT, (char *)&opt, sizeof(opt));
  if (rtn < 0) {
    AINFO << "setsockopt failed!!!";
    return false;
  }

  if (bind(sockfd_, reinterpret_cast<sockaddr *>(&my_addr), sizeof(sockaddr)) ==
      -1) {
    AERROR << "Socket bind failed! Port " << port_;
    return false;
  }

  if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
    AERROR << "non-block! Port " << port_;
    return false;
  }
  AINFO << "Lidar socket fd is " << sockfd_ << ", port: " << port_;
  return true;
}

int SocketInput::GetFiringDataPacket(HesaiLidarPacketData *pkt) {
  while (true) {
    if (!InputAvailable(POLL_TIMEOUT)) {
      return SOCKET_TIMEOUT;
    }
    // Receive packets that should now be available from the
    // socket using a blocking read.
    ssize_t nbytes =
        recvfrom(sockfd_, pkt, firing_data_packet_size_, 0, NULL, NULL);
    if (nbytes < 0) {
      if (errno != EWOULDBLOCK) {
        AERROR << "recvfail from port " << port_;
        return RECIEVE_FAIL;
      }
    }

    if ((size_t)nbytes == firing_data_packet_size_) {
      // read successful, done now
      uint64_t sys_time =
          std::chrono::high_resolution_clock::now().time_since_epoch().count();
      uint64_t pkt_time = GetPacketTimestamp(*pkt);
      if (sys_time - pkt_time > 40'000'000 &&
          pkt_time - sys_time > 40'000'000) {
        AINFO << "Hesai packet receive: SysTime " << sys_time
              << " PointCloudTime " << pkt_time;
      }
      break;
    }
    AERROR << "Incomplete Lidar rising data packet read: " << nbytes
           << " from port " << port_;
  }
  return 0;
}

bool SocketInput::InputAvailable(int timeout) {
  (void)timeout;
  struct pollfd fds[1];
  fds[0].fd = sockfd_;
  fds[0].events = POLLIN;
  do {
    int retval = poll(fds, 1, POLL_TIMEOUT);

    if (retval < 0) {  // poll() error?
      if (errno != EINTR) {
        AERROR << "Lidar port " << port_ << " poll error: " << strerror(errno);
      }
      return false;
    }

    if (retval == 0) {  // poll() timeout?
      AERROR << " Lidar port " << port_ << " poll timeout";
      return false;
    }
    if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
        (fds[0].revents & POLLNVAL)) {  // device error?
      AERROR << "Lidar port " << port_ << " poll reports error";
      return false;
    }
  } while ((fds[0].revents & POLLIN) == 0);
  return true;
}
}  // namespace lidar_hesai
}  // namespace drivers
}  // namespace apollo
